Method of accurate mapping with mobile robots

ABSTRACT

A robotic mapping method includes scanning a robot across a surface to be mapped. Locations of a plurality of points on the surface are sensed during the scanning. A first of the sensed point locations is selected. A preceding subset of the sensed point locations is determined. The preceding subset is disposed before the first sensed point location along a path of the scanning. A following subset of the sensed point locations is determined. The following subset is disposed after the first sensed point location along the path of the scanning. The first sensed point location is represented in a map of the surface by an adjusted first sensed point location. The adjusted first sensed point location is closer to each of the preceding and following subsets of the sensed point locations than is the first sensed point location.

BACKGROUND OF THE INVENTION

1. Field of the Invention

The present invention relates to mobile robots, and, more particularly,to mapping techniques that are used with mobile robots.

2. Description of the Related Art

Automatic map building of environments, particularly indoorenvironments, is a basic issue of mobile robotics. Digitaltwo-dimensional and three-dimensional models of the environment areneeded in all applications with autonomous mobile robots. If a robotwere to have an a priori map, then localization would be a relativelyeasy task. Alternatively, if the robot were to have a precise,externally referenced position estimate, then mapping would be an easytask. However, problems in which the robot has no a priori map and noexternal position reference are particularly challenging. Such scenariosmay arise for autonomous underwater vehicles (AUVs), miningapplications, or planetary surfaces.

The process of robotic mapping involves acquiring spatial models ofphysical environments via the use of, and for the use of, mobile robots.Maps are often used for robot navigation, such as for localization, andin many other application areas. Robotic mapping techniques withoutexternal position reference are generally referred to as SLAM(simultaneous localization and mapping) or CML (concurrent mapping andlocalization).

Robots are known to include sensors that detect the positions of objectsand terrain encountered by the robot as it travels. The sensor maymeasure the distance between the robot and some object, or between therobot and the surface on which the robot is traveling. These sensorreadings are made and recorded relative to the body of the robot.However, the location and orientation of the robot at any given timecannot be precisely determined. If the poses and localization (i.e.,orientation and location) of a robot were known, the local outputs ofthe robot's sensor could be registered into a common coordinate systemto create a map. Unfortunately, any mobile robot's determination of itsown localization is liable to be imprecise. Because the robot is unableto determine where it is and what its orientation is precisely, itsability to create a map is limited. Moreover, because the robot cannotcreate an accurate map, its ability to determine its own location andorientation is also limited.

Early work in SLAM assumed that a map used for mobile robots could bemodeled as a discrete set of landmarks. Different kinds ofrepresentations, or maps, have been proposed in the robotics andartificial intelligence literature. These representations, or maps,range from low-level metric maps, such as landmark maps and occupancygrids, to topological graphs that contain high level qualitativeinformation, or even multiple hierarchies of successively higher levelabstractions.

Traditionally, SLAM implementations based on Kalman filter data fusionhave relied on simple geometric models for defining landmarks. Thislimits landmark based algorithms to environments suited to such modelsand tends to discard a lot of potentially useful data. Only morerecently, Nieto et al. showed how to define landmarks composed of rawsensed data.

A limitation of almost all SLAM algorithms lies in the necessity toselect appropriate landmarks. By reducing the sensor data to thepresence of landmarks, a lot of information originally retrieved by asensor is usually discarded.

Another problem that arises from using discrete landmarks in SLAM is theproblem of data association. Before fusing data into the map, newmeasurements are associated with existing map landmarks, which has beenproven crucial in practical SLAM implementations.

Yet another problem is that all map representations used in currentSLAM-based approaches assume a random structure on the map or on thefeatures in the map. In actuality, this is rarely the case, as man-madestructures are generally highly structured. The insides of buildings,which are common workplaces for mobile robots, are typically constructedwith well known methodology.

What is neither disclosed nor suggested in the art is a robotic mappingmethod that overcomes the above-described problems with known mappingmethods.

Known SLAM methods can be described in mathematical terms presentedhereinbelow. In this context, the methods of the present inventiondescribed herein may be more easily described and understood.

Known SLAM methods have been designed with the goal of simultaneouslyestimating both the robot's pose and the map. A SLAM algorithm creates amap while at the same time it estimates the position of the robot insidethe map. When a robot operates inside of a building, planartwo-dimensional maps are usually sufficient. If, however, a robot has tooperate with additional degrees of freedom, then a three-dimensional mapmay need to be constructed.

At a time t, the following quantities are defined:

-   -   x_(t): A vector describing the position and orientation of the        robot.    -   u_(t): The control vector that was applied at time t−1 and        carries information about the change of the robot's pose.    -   z_(t) ^(k): The observations taken from the robot of the k^(th)        feature.    -   c_(t) ^(k): A correspondence variable between the k^(th) feature        and the i^(th) landmark.    -   m: A vector of features m={m_(i)} representing the environment        around the robot.

A more detailed definition of these quantities in the context of aprobabilistic model is provided hereinbelow. In probabilistic SLAM, thisis often posed as Bayesian filtering formulation. Two main forms of sucha formulation exist. One is known as the “online SLAM problem”:

p(x_(t),m|u_(1:t),z_(1:t),c_(1:t))

It involves estimating the posterior over the momentary pose x_(t) alongwith the map m. It is called “online” because it involves onlyestimating quantities at the time t. In contrast, the “full SLAMproblem” seeks to calculate a posterior over all quantities:

p(x_(1:t),m|u_(1:t),z_(1:t),c_(1:t))  (2)

The full SLAM problem is focused on herein in order to simplify thefurther discussion. A closed form expression of Equation (2) can beobtained by recursively applying Bayes' Rule and subsequent inductionover t:

p(x _(1:t) ,m|u _(1:t) ,z _(1:t) ,c _(1:t))=ηp(x ₀)p(m)Π_(t) [p(x _(t)|x _(t-1) ,u _(t))Π_(i) p(z _(t) ^(i) |x _(t) ,m,c _(t) ^(i))]  (3)

Here p(x_(t)|x_(t-1),u_(t)) is known as the “motion model” whichdescribes state transitions of the robot's pose in terms of aprobability distribution. The state transitions are assumed to be aMarkov process and independent of both the observations and the map. Theterm p(z_(t) ^(i)|x_(t),m,c_(t) ^(i)) on the other hand denotes an“observation model” which models an observation z_(t) ^(i) from a knownpose and a known map as a probability distribution. Both models havebeen studied well for a variety of robots and sensors. The two priorterms p(x₀) and p(m) characterize priors about the first robot pose andabout the map, respectively. Usually p(x₀) is used to anchor the initialpose to a fixed location. The map prior p(m) is typically assumed to beunknown and subsumed into the normalizer η. The process of finding themost probable solution to the full SLAM problem is simply the process offinding the set of poses {circumflex over (x)}_(1:t) and the map{circumflex over (m)} that maximizes the posterior probability ofEquation (3).

$\begin{matrix}{{\hat{x}}_{l:t},{\hat{m} = {\underset{x_{l:t},m}{argmax}{p\left( {x_{l:t},{mu_{l:t}},z_{l:t},c_{l:t}} \right)}}}} & (4)\end{matrix}$

A graphical model of this formulation is illustrated in FIG. 1 a whichshows the Bayes network of traditional landmark based SLAM. Eachobservation z_(t) ^(k) is associated with a map feature m_(i). Thenon-divergence property is achieved since map features are observed fromseveral locations. Correlations between map features are not considered.

SUMMARY OF THE INVENTION

The present invention provides a method for reconstructing a map basedon range measurements from a mobile platform and creating accurate mapsof indoor environments. The method includes a novel probabilisticformulation which does not rely on data correspondences. Instead ofusing occupancy grid maps to represent the environment, geometric maprepresentations and statistical prior models may be used to reconstructmaps from the data collected by a mobile robot.

More particularly, the present invention provides a method ofregistering multiple surface scans in a common coordinate frame andreconstructing the scanned surface at the same time. The inventionfurther provides a novel probabilistic technique for solving the offlineSLAM problem by jointly solving the data registration problem and thefaithful reconstruction of the underlying geometry. Prior knowledge isincorporated in the map that is being constructed.

Although the typical landmark SLAM model assumes the environment isunstructured, i.e., that landmarks are randomly and independentlydistributed in the environment, the method of the present invention maynot make this assumption. In the present invention, the measurements maybe considered directly without processing them into any landmarks. Itmay be assumed that there is not any immediate correspondence betweenthe measurements and the landmarks. This assumption may be quitereasonable for a number of situations. For example, a mobile robotequipped with light detection and ranging (LIDAR), which takes a finitenumber of measurements while it is in motion, is very unlikely tomeasure the exact same spot twice.

According to the present invention, each observation may create a newfeature in the map. It may be assumed that there are no correspondencesbetween observations and known features. Instead, the map prior may beused to estimate the robot's pose and the map.

The map prior may be expressed as a probability distribution whichrepresents a prior distribution of all measured scenes. An exactprobabilistic model of this distribution may be infeasible and probablynot even well defined. Hence, the present invention may focus on partialmodels which represent properties of the surface structure. For theoptimization of the SLAM problem, a good surface prior may be veryhelpful. The observation model, the motion model, as well as the prioron the first pose may be at their equilibrium by using the measurements.This means that without any map prior, the most probable solution may bethe measurement itself. According to the present invention, priors maybe used representing three properties: manifold priors, smoothnesspriors and priors for the orientation.

The present invention may be used in the fields of robotics, mapping and3D reconstruction. Due to the intrinsic limitations of sensor systems,spatial sensor interpretation is fundamentally an underconstrainedproblem. Incorporating simple priors on the environment according to thepresent invention enables a robot to recover better world models. Thepresent invention provides a novel formulation of the SLAM problem whichincorporates map priors and does not rely on the notion of landmarks.

The invention comprises, in one form thereof, a robotic mapping methodincluding scanning a robot across a surface to be mapped. Locations of aplurality of points on the surface are sensed during the scanning. Afirst of the sensed point locations is selected. A first subset of thesensed point locations that are within a vicinity of the first sensedpoint location is determined. A line segment that approximates the firstsubset of sensed point locations is ascertained. The first sensed pointlocation is represented in a map of the surface by an adjusted firstsensed point location. The adjusted first sensed point location iscloser to the line segment than is the first sensed point location.

The invention comprises, in another form thereof, a robotic mappingmethod including scanning a robot across a surface to be mapped.Locations of a plurality of points on the surface are sensed during thescanning. A first of the sensed point locations is selected. A precedingsubset of the sensed point locations is determined. The preceding subsetis disposed before the first sensed point location along a path of thescanning. A following subset of the sensed point locations isdetermined. The following subset is disposed after the first sensedpoint location along the path of the scanning. The first sensed pointlocation is represented in a map of the surface by an adjusted firstsensed point location. The adjusted first sensed point location iscloser to each of the preceding and following subsets of the sensedpoint locations than is the first sensed point location.

The invention comprises, in yet another form thereof, a robotic mappingmethod including scanning at least one robot across a surface to bemapped. The scanning includes a first scan and a second scan. Locationsof a plurality of points on the surface are sensed. A first set of thesensed point locations are sensed during the first scan, and a secondset of the sensed point locations are sensed during the second scan. Afirst pair of adjacent sensed point locations from the first set isselected. A first imaginary line segment joining the first pair ofadjacent sensed point locations has a first slope. A second pair ofadjacent sensed point locations from the second set is selected. Thefirst pair of adjacent sensed point locations has a position in adirection of the scanning that corresponds to a position of the secondpair of adjacent sensed point locations in the direction of thescanning. A second imaginary line segment joining the second pair ofadjacent sensed point locations has a second slope. One of the firstpair of adjacent sensed point locations is represented in a map of thesurface by an adjusted first sensed point location. A third imaginaryline segment joins the adjusted first sensed point location and an othersensed point location of the first pair. The third imaginary linesegment has a third slope. The third slope is closer to the second slopethan is the first slope.

The invention comprises, in still another form thereof, a roboticmapping method including using a robot to sense locations of a pluralityof points on a surface to be mapped. A first of the sensed pointlocations is represented in a map of the surface by an adjusted firstsensed point location. The adjusted first sensed point location isdependent upon at least a second and a third of the sensed pointlocations.

An advantage of the present invention is that it reconstructs maps thatare significantly more accurate than those produced by prior artalgorithms. The maps produced by the present invention resemble theactual structure with a much higher level of detail. The geometric maprepresentations that are used yield higher accuracy and scale betterthan grid maps. When applied to SLAM, the method of the invention findsmaps that closely resemble the real environment. The inventive method isparticularly superior to the prior art in cases in which no salientlandmark definition is feasible.

BRIEF DESCRIPTION OF THE DRAWINGS

The above mentioned and other features and objects of this invention,and the manner of attaining them, will become more apparent and theinvention itself will be better understood by reference to the followingdescription of embodiments of the invention taken in conjunction withthe accompanying drawings, wherein:

FIG. 1 a is a diagram illustrating a Bayes network of landmark basedSLAM of the prior art.

FIG. 1 b is a network according to one embodiment of a mapping method ofthe present invention.

FIG. 2 a is a diagram illustrating one step of one embodiment of amanifold prior utilized in a mapping method of the present invention.

FIG. 2 b is a diagram illustrating another step of the manifold prior ofFIG. 2 a.

FIG. 2 c is a diagram illustrating yet another step of the manifoldprior of FIG. 2 a.

FIG. 3 a is a diagram illustrating one step of one embodiment of a shapeprior utilized in a mapping method of the present invention.

FIG. 3 b is a diagram illustrating another step of the shape prior ofFIG. 3 a.

FIG. 3 c is a diagram illustrating yet another step of the shape priorof FIG. 3 a.

FIG. 4 a is a diagram illustrating one step of one embodiment of anorientation prior utilized in a mapping method of the present invention.

FIG. 4 b is a diagram illustrating another step of the orientation priorof FIG. 4 a.

FIG. 4 c is a diagram illustrating yet another step of the orientationprior of FIG. 4 a.

FIG. 5 is a flow chart of one embodiment of a mapping method of thepresent invention.

FIG. 6 is a flow chart of another embodiment of a mapping method of thepresent invention.

FIG. 7 is a flow chart of yet another embodiment of a mapping method ofthe present invention.

Corresponding reference characters indicate corresponding partsthroughout the several views. Although the exemplification set outherein illustrates embodiments of the invention, in several forms, theembodiments disclosed below are not intended to be exhaustive or to beconstrued as limiting the scope of the invention to the precise formsdisclosed.

DESCRIPTION OF THE PRESENT INVENTION

In the formulation of the present invention, some prior knowledge aboutmap m is assumed to be available, and this knowledge may be used toreformulate the SLAM problem. First, according to the invention, thenotion of landmarks may be eliminated. In prior art formulations, it isassumed that the correspondences c_(t) ^(k) are known beforehand, whichenables a landmark m_(i) to be uniquely assigned to each observedfeature. However, in practical prior art SLAM implementations, thisbecomes a crucial step. It is very challenging to find the correctcorrespondences, and a single incorrect correspondence often causes acatastrophic failure. In the formulation of the present invention, themeasurements are considered directly without processing them into anylandmarks, the correspondence to such landmarks not being known.Instead, according to the invention, there is no immediatecorrespondence between measurements. In fact, this assumption may beaccurate for a many situations. For example, a mobile robot equippedwith LIDAR may take a finite number of measurements or “observations”while the robot is in motion. It is very unlikely that the robot everobserves the exact same location more than once.

The present invention has many differences from the SLAM formulations ofthe prior art, including:

-   -   Each observation z_(t) ^(k) creates a new feature in the map.    -   No correspondence is assumed between observations and known        features.    -   Instead, the map prior p(m) is used to estimate the robot's pose        and the map.

The new posterior for this formulation is:

p(x _(1:t) ,m|u _(1:t) ,z _(1:t))=ηp(x ₀)p(m)Π_(t) [p(x _(t) |x _(t-1),u _(t))Π_(i) p(z _(t) ^(i) |x _(t) ,m)]  (5)

A graphical model of this formulation is illustrated in FIG. 1 b whichshows the network according to a method of the present invention. Incontrast to the prior art method depicted in FIG. 1 a, it is not assumedthat map features are observed from different locations. Instead, thecorrelations between features are used to achieve a non-divergenceproperty.

The different components of Equation (5) are now described in moredetail. The probability distribution p(x_(t)|x_(t-1),u_(t)) for themotion model can also be expressed as a Gaussian distribution

$\begin{matrix}{{p\left( {{x_{t}x_{t - 1}},u_{t}} \right)} \propto {\exp \left\{ {\frac{1}{2}\left( {x_{t} - {g\left( {u_{t},x_{t - 1}} \right)}} \right)^{T}{R_{t}^{- 1}\left( {x_{t} - {g\left( {u_{t},x_{t - 1}} \right)}} \right)}} \right\}}} & (6)\end{matrix}$

Here g(u_(t), x_(t-1)) denotes a function that models the statetransition. The common model is used where the robot is assumed toperform a rotation δ_(φ,1) followed by a translation δ_(t), followed byanother translation δ_(φ,2):

$\begin{matrix}{{g\left( {u_{t},x_{t - 1}} \right)} = {\begin{pmatrix}x_{{t - 1},x} \\x_{{t - 1},y} \\x_{{t - 1},\theta}\end{pmatrix} + \begin{pmatrix}{\delta_{t}{\cos \left( {\theta + \delta_{\varphi,1}} \right)}} \\{\delta_{t}{\sin \left( {\theta + \delta_{\varphi,1}} \right)}} \\{\delta_{\varphi,1} + \delta_{\varphi,2}}\end{pmatrix}}} & (7)\end{matrix}$

In this case, this function is a linear approximation of the true motionfunction g

g(u _(t) ,x _(t-1))≈ g (ũ_(t),x_(t-1))+G _(t)(x _(t-1) −{tilde over (x)}_(t-1))  (8)

at the expected pose {tilde over (x)}_(t-1) and with the controlparameters ũ_(t). The Jacobian G_(t) is the derivative of the function gwith respect to x_(t-1).

For exact motion, the incremental travel distance in the time intervalΔt between any two poses

$\begin{matrix}{u_{t} = {\begin{pmatrix}{x^{\prime} - x} \\{y^{\prime} - y} \\{\theta^{\prime} - \theta}\end{pmatrix} = \begin{pmatrix}\delta_{x} \\\delta_{y} \\\delta_{o}\end{pmatrix}}} & (9)\end{matrix}$

is represented by a concatenation of three basic motions: a rotationδ_(φ,1), a straight-line motion δ_(t), and another rotation δ_(φ,2)

δ_(φ,1)=α tan 2(δ_(y)−δ_(x))  (10)

δ_(t)=√{square root over (δ_(x) ²+δ_(y) ²)}  (11)

δ_(φ,2)=δ_(θ)−δ_(φ,1)  (12)

Consequently, the true position after one timestamp Δt can be obtainedby

$\begin{matrix}{\begin{pmatrix}x^{\prime} \\y^{\prime} \\\theta^{\prime}\end{pmatrix} = {\begin{pmatrix}x \\y \\\theta\end{pmatrix} + \begin{pmatrix}{\delta_{t}{\cos \left( {\theta + \delta_{\varphi,1}} \right)}} \\{\delta_{t}{\sin \left( {\theta + \delta_{\varphi,1}} \right)}} \\{\delta_{\varphi,1} + \delta_{\varphi,2}}\end{pmatrix}}} & (13) \\{\mspace{56mu} {= {\begin{pmatrix}x \\y \\\theta\end{pmatrix} + \begin{pmatrix}{\sqrt{\delta_{x}^{2} + \delta_{y}^{2}}{\cos \left( {\theta + {a\; \tan \; 2\left( {\delta_{y} - \delta_{x}} \right)}} \right)}} \\{\sqrt{\delta_{x}^{2} + \delta_{y}^{2}}{\sin \left( {\theta + {a\; \tan \; 2\left( {\delta_{y} - \delta_{x}} \right)}} \right)}} \\\delta_{\theta}\end{pmatrix}}}} & (14)\end{matrix}$

In actuality, real platform motion is subject to noise. The values ofthe rotations and the translation are disturbed by independent noise.According to the invention, this noise may be modeled by a zero-centeredrandom variable with finite variance. It may be assumed that the actualincremental travel distances are given by

$\begin{matrix}{\begin{pmatrix}{\hat{\delta}}_{\varphi,1} \\{\hat{\delta}}_{t} \\{\hat{\delta}}_{\varphi,2}\end{pmatrix} = {{\begin{pmatrix}\delta_{\varphi,1} \\\delta_{t} \\\delta_{\varphi,2}\end{pmatrix} + \begin{pmatrix}ɛ_{{a_{1}{\delta_{\varphi,1}}} + {a_{2}{\delta_{t}}}} \\ɛ_{{a_{3}{\delta_{t}}} + {a_{4}{{\delta_{\varphi,1} + \delta_{\varphi,2}}}}} \\ɛ_{{a_{1}{\delta_{\varphi,2}}} + {a_{2}{\delta_{t}}}}\end{pmatrix}} = {\begin{pmatrix}\delta_{\varphi,1} \\\delta_{t} \\\delta_{\varphi,2}\end{pmatrix} + {{\left( {0,M_{t}} \right)}.}}}} & (15)\end{matrix}$

Here ε_(σ) is a zero-mean, normal distributed error variable withstandard deviation σ.

$\begin{matrix}{{ɛ_{\sigma}(x)} = {\frac{1}{\sqrt{2{\pi\sigma}^{2}}}^{{- \frac{1}{2}}\frac{s^{2}}{\sigma^{2}}}}} & (16)\end{matrix}$

and M_(t) is the noise covariance matrix in motion space

$\begin{matrix}{M_{t} = {\begin{pmatrix}\left( {{\alpha_{1}{\delta_{\varphi,1}}} + {\alpha_{2}{\delta_{t}}}} \right)^{2} & 0 & 0 \\0 & \begin{matrix}\left( {{\alpha_{3}{\delta_{t}}} + {\alpha_{4}{{\delta_{\varphi,1} +}}}} \right. \\\left. {\delta_{\varphi,2}} \right)^{2}\end{matrix} & 0 \\0 & 0 & \left( {{\alpha_{1}{\delta_{\varphi,2}}} + {\alpha_{2}{\delta_{t}}}} \right)^{2}\end{pmatrix}.}} & (17)\end{matrix}$

In the model of the present invention, the standard deviation of theerror may be proportional to the rotations and the translation. Theparameters α₁, α₂, α₃ and α₄ are platform-specific error parameters. Abetter model of the actual pose after a time interval Δt is thus

$\begin{matrix}{\begin{pmatrix}x^{\prime} \\y^{\prime} \\\theta^{\prime}\end{pmatrix} = {\begin{pmatrix}x \\y \\\theta\end{pmatrix} + \begin{pmatrix}{{\hat{\delta}}_{t}{\cos \left( {\theta + {\hat{\delta}}_{\varphi,1}} \right)}} \\{{\hat{\delta}}_{t}{\sin \left( {\theta + {\hat{\delta}}_{\varphi,1}} \right)}} \\{{\hat{\delta}}_{\varphi,1} + {\hat{\delta}}_{\varphi,2}}\end{pmatrix}}} & (18)\end{matrix}$

For further use, the motion model may be linearized. For that, the modelmay be decomposed into a noise-free part and a random noise component

$\begin{matrix}{\underset{\underset{x_{t}}{}}{\begin{pmatrix}x^{\prime} \\y^{\prime} \\\theta^{\prime}\end{pmatrix}} = {\underset{\underset{g{({u_{t},x_{t - 1}})}}{}}{\underset{\underset{x_{t - 1}}{}}{\begin{pmatrix}x \\y \\\theta\end{pmatrix}} + \begin{pmatrix}{\delta_{t}{\cos \left( {\theta + \delta_{\varphi,1}} \right)}} \\{\delta_{t}{\sin \left( {\theta + \delta_{\varphi,1}} \right)}} \\{\delta_{\varphi,1} + \delta_{\varphi,2}}\end{pmatrix}} + {{\left( {0,R_{t}} \right)}.}}} & (19)\end{matrix}$

Equation 19 approximates Equation 18 by replacing the true motion({circumflex over (δ)}_(φ,2) {circumflex over (δ)}_(t) {circumflex over(δ)}_(φ,2))^(T) by the measured motion (δ_(φ,1) δ_(t) δ_(φ,2)) andcapturing the motion noise in an additive Gaussian with zero mean. Thefunction g may be approximated through a Taylor expansion

g(u _(t) ,x _(t-1))≈g(ũ _(t) ,{tilde over (x)} _(t-1))+G _(t)(x _(t-1)−{tilde over (x)} _(t-1))  (20)

The Jacobian G, is the derivative of the function g with respect tox_(t-1) at the expected pose {tilde over (x)}_(t-1)=({tilde over (x)}{tilde over (y)} {tilde over (θ)})^(T) and with the expected motionparameters ũ_(t)({circumflex over (δ)}_(φ,1) {circumflex over (δ)}_(t){circumflex over (δ)}_(φ,2))^(T)

$\begin{matrix}\begin{matrix}{G_{t} = \frac{\partial{g\left( {{\overset{\sim}{u}}_{t},{\overset{\sim}{x}}_{t - 1}} \right)}}{\partial x_{t - 1}}} \\{= \begin{pmatrix}\frac{\partial g_{x^{\prime}}}{\partial\overset{\sim}{x}} & \frac{\partial g_{x^{\prime}}}{\partial\overset{\sim}{y}} & \frac{\partial g_{x^{\prime}}}{\partial\overset{\sim}{\theta}} \\\frac{\partial g_{y^{\prime}}}{\partial\overset{\sim}{x}} & \frac{\partial g_{y^{\prime}}}{\partial\overset{\sim}{y}} & \frac{\partial g_{y^{\prime}}}{\partial\overset{\sim}{\theta}} \\\frac{\partial g_{\theta^{\prime}}}{\partial\overset{\sim}{x}} & \frac{\partial g_{\theta^{\prime}}}{\partial\overset{\sim}{y}} & \frac{\partial g_{\theta^{\prime}}}{\partial\overset{\sim}{\theta}}\end{pmatrix}}\end{matrix} & (21) \\{\mspace{25mu} {= {\begin{pmatrix}1 & 0 & {{- {\overset{\sim}{\delta}}_{t}}{\sin \left( {\theta + {\overset{\sim}{\delta}}_{\varphi,1}} \right)}} \\0 & 1 & {{\overset{\sim}{\delta}}_{t}{\cos \left( {\theta + {\overset{\sim}{\delta}}_{\varphi,1}} \right)}} \\0 & 0 & 1\end{pmatrix}.}}} & (22)\end{matrix}$

The motion model in Equation 19 may require the motion noise M, to bemapped into “state space.” This may once again be done by linearapproximation. The Jacobian needed for this approximation, denotedV_(t), is the derivative of the function g with respect to the motionparameters u_(t) at the expected pose {tilde over (x)}_(t-1)=({tildeover (x)} {tilde over (y)} {tilde over (θ)})^(T) and with the expectedmotion parameters ũ_(t)({circumflex over (δ)}_(φ,1) {circumflex over(δ)}_(t) {circumflex over (δ)}_(φ,2))^(T)

$\begin{matrix}\begin{matrix}{V_{t} = \frac{\partial{g\left( {{\overset{\sim}{u}}_{t},{\overset{\sim}{x}}_{t - 1}} \right)}}{\partial{\overset{\sim}{u}}_{t}}} \\{= \begin{pmatrix}\frac{\partial g_{x^{\prime}}}{\partial{\overset{\sim}{\delta}}_{\theta,1}} & \frac{\partial g_{x^{\prime}}}{\partial{\overset{\sim}{\delta}}_{t}} & \frac{\partial g_{x^{\prime}}}{\partial{\overset{\sim}{\delta}}_{\theta,2}} \\\frac{\partial g_{y^{\prime}}}{\partial{\overset{\sim}{\delta}}_{\theta,1}} & \frac{\partial g_{y^{\prime}}}{\partial{\overset{\sim}{\delta}}_{t}} & \frac{\partial g_{y^{\prime}}}{\partial{\overset{\sim}{\delta}}_{\theta,2}} \\\frac{\partial g_{\theta^{\prime}}}{\partial{\overset{\sim}{\delta}}_{\theta,1}} & \frac{\partial g_{\theta^{\prime}}}{\partial{\overset{\sim}{\delta}}_{t}} & \frac{\partial g_{\theta^{\prime}}}{\partial{\overset{\sim}{\delta}}_{\theta,2}}\end{pmatrix}}\end{matrix} & (23) \\{\mspace{20mu} {= \begin{pmatrix}{{- {\overset{\sim}{\delta}}_{t}}{\sin \left( {\overset{\sim}{\theta} + {\overset{\sim}{\delta}}_{\varphi,1}} \right)}} & {\cos \left( {\overset{\sim}{\theta} + {\overset{\sim}{\delta}}_{\varphi,1}} \right)} & 0 \\{{\overset{\sim}{\delta}}_{t}{\cos \left( {\overset{\sim}{\theta} + {\overset{\sim}{\delta}}_{\varphi,1}} \right)}} & {\sin \left( {\overset{\sim}{\theta} + {\overset{\sim}{\delta}}_{\varphi,1}} \right)} & 0 \\1 & 0 & 1\end{pmatrix}}} & (24)\end{matrix}$

Finally, the approximate mapping between the motion noise in motionspace to the motion noise in state space is derived by themultiplication

R_(t)=V_(t)M_(t)V_(t) ^(T).  (25)

The probability distribution p(z_(t) ^(i)|x_(t), m) for the observationmodel can also be expressed as Gaussian distribution

$\begin{matrix}{{p\left( {{z_{t}^{i}x_{t}},m_{i}} \right)} \propto {\exp \left\{ {\frac{1}{2}\left( {z_{t}^{i} - {h\left( {x_{t},m_{i}} \right)}} \right)^{T}{Q_{t,i}^{- 1}\left( {z_{t}^{i} - {h\left( {x_{t},m_{i}} \right)}} \right)}} \right\}}} & (26)\end{matrix}$

wherein h(x_(t), m) denotes a function that models the observation z_(t)^(i). A beam model may be used for sensors that measure range indifferent directions:

$\begin{matrix}{{h\left( {x_{t},m_{i}} \right)} = \begin{pmatrix}\sqrt{\left( {m_{i,x} - x_{t,x}} \right)^{2} + \left( {m_{i,y} - x_{t,y}} \right)^{2}} \\{{a\; \tan \; 2\left( {{m_{i,y} - x_{t,y}},{m_{i,x} - x_{t,x}}} \right)} - x_{t,\theta}}\end{pmatrix}} & (27)\end{matrix}$

Again, this function is a linear approximation of the true measurementfunction h

$\begin{matrix}{{h\left( {x_{t},m_{i}} \right)} \approx {{\overset{\_}{h}\left( {{\overset{\sim}{x}}_{t},{\overset{\sim}{m}}_{i}} \right)} + {H_{t,i}\begin{pmatrix}{x_{t} - {\overset{\sim}{x}}_{t}} \\{m_{i} - {\overset{\sim}{m}}_{i}}\end{pmatrix}}}} & (28)\end{matrix}$

at the expected pose {tilde over (x)}_(t) and observing the expectedfeature {tilde over (m)}_(i). The Jacobian H_(t,i) is the derivative ofthe function h with respect to x_(t) and m_(i).

A sensor may measure the two-dimensional coordinates of a surface

$\begin{matrix}{{m_{n} = \begin{pmatrix}m_{x} \\m_{y}\end{pmatrix}},} & (29)\end{matrix}$

where x_(m) and y_(m) are the true coordinates in a global coordinatesystem. The beam model approximates the physical model of range finders.Range finders measure the range r_(m), along a beam, which originates atthe local coordinate system of the sensor. The angular orientation ofthe sensor beam is denoted as θ_(m)

$\begin{matrix}{z_{l}^{k} = \begin{pmatrix}r_{m} \\\theta_{m}\end{pmatrix}} & (30)\end{matrix}$

The endpoint of this measurement is mapped into the global coordinatesystem via the trigonometric transformation

$\begin{matrix}{\begin{pmatrix}r_{m} \\\theta_{m}\end{pmatrix} = \begin{pmatrix}\sqrt{\left( {m_{x} - x} \right)^{2} + \left( {m_{y} - y} \right)^{2}} \\{{atan}\; 2\left( {{m_{y} - y},{m_{x} - x}} \right)}\end{pmatrix}} & (31)\end{matrix}$

where x_(t)=(x y θ)^(T) denotes the pose of the sensing platform inglobal coordinates.

In actuality, real measurements are subject to noise. The values of therange and the angular orientation are disturbed by independent noise.This noise may be modeled by a zero-centered random variable with finitevariance. Assuming that the actual measurements are given by

$\begin{matrix}{\begin{pmatrix}{\hat{r}}_{m} \\{\hat{\theta}}_{m}\end{pmatrix} = {{\begin{pmatrix}r_{m} \\\theta_{m}\end{pmatrix} + \begin{pmatrix}ɛ_{\alpha_{1}} \\ɛ_{\alpha 2}\end{pmatrix}} = {\begin{pmatrix}r_{m} \\\theta_{m}\end{pmatrix} + {\left( {0,Q_{t}} \right)}}}} & (32)\end{matrix}$

and Q_(t) is the noise covariance matrix in “measurement space”

$\begin{matrix}{Q_{t} = {\begin{pmatrix}\alpha_{1}^{2} & 0 \\0 & \alpha_{2}^{2}\end{pmatrix}.}} & (33)\end{matrix}$

A better model of the actual measurement is thus

$\begin{matrix}{z_{t}^{k} = {\begin{pmatrix}{\hat{r}}_{m} \\{\hat{\theta}}_{m}\end{pmatrix} = {\underset{h{({m_{n},x_{t}})}}{\underset{}{\begin{pmatrix}\sqrt{\left( {m_{x} - x} \right)^{2} + \left( {m_{y} - y} \right)^{2}} \\{{{atan}\; 2\left( {{m_{y} - y},{m_{x} - x}} \right)} - \theta}\end{pmatrix}}} + {\left( {0,Q_{t}} \right)}}}} & (34)\end{matrix}$

For further use, the measurement model may be linearized. The function hmay be approximated through a Taylor expansion

$\begin{matrix}{{h\left( {m_{n},x_{t}} \right)} \approx {{h\left( {{\overset{\sim}{m}}_{n},{\overset{\sim}{x}}_{t}} \right)} + {{H_{t}\begin{pmatrix}{x - \overset{\sim}{x}} \\{y - \overset{\sim}{y}} \\{\theta - \overset{\sim}{\theta}} \\{m_{x} - {\overset{\sim}{m}}_{x}} \\{m_{y} - {\overset{\sim}{m}}_{y}}\end{pmatrix}}.}}} & (35)\end{matrix}$

The Jacobian H_(t) is the derivative of the function h with respect tox_(t) at the current pose estimate {tilde over (x)}_(t)=({tilde over(x)} {tilde over (y)} {tilde over (θ)})^(T) and the surface estimate{tilde over (m)}_(t)=({tilde over (m)}_(x) {tilde over (m)}_(y))^(T)

$\begin{matrix}{H_{t} = {\frac{\partial{h\left( {{\overset{\sim}{m}}_{n},{\overset{\sim}{x}}_{t}} \right)}}{{\partial m_{n}},x_{t}} = \begin{pmatrix}\frac{\partial{\overset{.}{r}}_{m}}{\partial\overset{\sim}{x}} & \frac{\partial{\overset{.}{r}}_{m}}{\partial\overset{\sim}{y}} & \frac{\partial{\overset{.}{r}}_{m}}{\partial\overset{\sim}{\theta}} & \frac{\partial{\overset{.}{r}}_{m}}{\partial{\overset{\sim}{m}}_{x}} & \frac{\partial{\overset{.}{r}}_{m}}{\partial{\overset{\sim}{m}}_{y}} \\\frac{\partial{\hat{\theta}}_{m}}{\partial\overset{\sim}{x}} & \frac{\partial{\hat{\theta}}_{m}}{\partial\overset{\sim}{y}} & \frac{\partial{\hat{\theta}}_{m}}{\partial\overset{\sim}{\theta}} & \frac{\partial{\hat{\theta}}_{m}}{\partial{\overset{\sim}{m}}_{x}} & \frac{\partial{\hat{\theta}}_{m}}{\partial{\overset{\sim}{m}}_{y}}\end{pmatrix}}} & (36) \\{{= {\frac{1}{q}\begin{pmatrix}{\sqrt{q}{\overset{\sim}{\delta}}_{x}} & {{- \sqrt{q}}{\overset{\sim}{\delta}}_{y}} & 0 & {{- \sqrt{q}}{\overset{\sim}{\delta}}_{x}} & {\sqrt{q}\overset{\sim}{\delta}} \\{\overset{\sim}{\delta}}_{y} & {\overset{\sim}{\delta}}_{x} & {- q} & {- {\overset{\sim}{\delta}}_{y}} & {- {\overset{\sim}{\delta}}_{x}}\end{pmatrix}}}\mspace{20mu}} & (37) \\\; & (38) \\{{{{with}\mspace{14mu} q} = {{\overset{\sim}{\delta}}_{x}^{2} + {\overset{\sim}{\delta}}_{y}^{2}}},{{\overset{\sim}{\delta}}_{x} = {{\overset{\sim}{m}}_{x} - \overset{\sim}{x}}},{{{and}\mspace{14mu} {\overset{\sim}{\delta}}_{y}} = {{\overset{\sim}{m}}_{y} - {\overset{\sim}{y}.}}}} & (39)\end{matrix}$

The prior p(x₀) may anchor the first pose at its position (e.g., theorigin of the global coordinate system). The prior is easily expressedby a Gaussian-type distribution

$\begin{matrix}{{p\left( x_{0} \right)} = {{\eta exp}\left\{ {\frac{1}{2}\left( {x_{0} - {\overset{\sim}{x}}_{0}} \right)^{T}{\Omega_{0}^{- 1}\left( {x_{0} - {\overset{\sim}{x}}_{0}} \right)}} \right\}}} & (40)\end{matrix}$

wherein {tilde over (x)}₀ is the initial estimate of the first pose. Thecovariance Ω₀ is a simple matrix with values close to zero on thediagonal and zero everywhere else.

The probability distribution p(m) in Equation (5) represents a priordistribution of all measured scenes. An exact probabilistic model ofthis distribution may be infeasible and probably not even well defined.Hence, the focus of the present invention may be on partial models,which represent properties of the surface structure. For theoptimization of Equation (5), a good surface prior may be verybeneficial. The observation model, the motion model, as well as theprior on the first pose are at their equilibrium by using themeasurements. This means that without any map prior, the most probablesolution would be the measurement itself. The present invention usespriors representing three properties: manifold priors, smoothness priorsand priors for the orientation. It may be assumed that all three priorsare independent of each other, which leads to:

p(m)=p _(m)(m)p _(s)(m)p _(o)(m).  (41)

The independence assumption is most likely not true and is a goodstarting point for further improvements. It has been found empiricallythat even though this assumption is violated, the method of the presentinvention yields good results.

The three types of priors (i.e., manifold, smoothness and orientation)are described in detail hereinbelow. In FIGS. 2-4, there are shownspecific embodiments of the three different types of priors that may beutilized according to mapping methods of the present invention. Moreparticularly, a manifold prior is illustrated in FIGS. 2 a-c; a shapeprior is illustrated in FIGS. 3 a-c; and an orientation prior isillustrated in FIGS. 4 a-c. Each of the three priors is shown in FIGS.2-4 as being applied to three consecutive observed readings in order topresent an illustrative sample. However, it is to be understood thatthese priors may be applied to each available reading that has beenobserved. Moreover, any combination of the three types of priors may beapplied to the same set of data.

The intuition of the manifold prior is that the collected observationsbelong to surfaces in the robot's environment. For a two-dimensionalmap, this means that the most probable surface must be a compact,connected, one-dimensional manifold, possibly with a boundary embeddedin

².

FIG. 2 a includes a number of circles A-M each of which represents arespective observation or measurement of a surface location by a robotas the robot traverses the surface. It is possible that circles A-M arethe result of observations taken in a single scan or in multiples scansor trips of the robot across the surface. It is also possible thatcircles A-M are the result of observations taken while the robot movesat an angle, or even randomly, with respect to the surface. Thus, theremay be noise in observation points A-M due to uncertainty in matching upthe positions of the robot in the x-direction (i.e., in the directionacross the page of FIG. 2 a) when the robot made the observations. Thesurface may be substantially perpendicular to the page of FIG. 2 a, andthe robot may be displaced from the wall in the general direction ofarrow 10.

Each of observation points A-M may be in the same plane, as shown inFIG. 2 a. The map, the path of the robot, and the observations may bepurely two-dimensional. That is, the offsets may be in only the x and ydirections, i.e., in the plane of the page. Although each of observationpoints A-M may have its measured location adjusted according to themanifold prior technique as described below, FIG. 2 a more particularlyillustrates an adjustment of point F, which is arbitrarily selected forpurposes of the initial illustration.

A fixed neighborhood of point F may be defined by a circle 12 havingpoint F at the center of circle 12. The radius of circle 12 may bevariable within the scope of the invention, and the inventive manifoldtechnique is not limited to use of circles of any particular radius.However, in one embodiment, there are approximately between ten andtwenty points within circle 12. Moreover, a point's neighborhood may bedefined by shapes other than circular within the scope of the invention.The points within the neighborhood, i.e. points C, D, E, F, G, and Hwithin circle 12, may be used to create a best fit line 14, such as aleast squares regression line. That is, line 14 may be calculated as theline that minimizes the sum of the squares of the distances of each ofpoints C, D, E, F, G, and H to the line.

The distribution of points C, D, E, F, G, and H around line 14 may bemodeled as a Gaussian distribution over the projected distance to thetangent line. That is, the Gaussian distribution may have its peak valueat line 14, and the distribution values may decline in directionsoutwardly away from (i.e., perpendicular to) line 14. The most probablearrangement regarding only this manifold prior is when all points arelocated on the same one-dimensional manifold.

According to the present invention, point F may be moved closer to line14, and in a direction 15 perpendicular to line 14, in the finalmapping. Thus, the location of point F after movement may besubstantially aligned with its original location in the verticaldirection. The degree to which, or distance by which, point F is movedtoward line 14 may be variable according to the invention, and theinvention is not intended to be limited to any particular method ofdetermining the amount of movement of point F toward line 14. In aspecific embodiment, however, point F is moved toward line 14 by avariable amount that is governed by the Gaussian distribution. Thus, thecloser a possible final destination is to line 14, the more likely it isthat point F will actually be moved to that final destination.

A normal n_(i) may be represented by an arrow superimposed on arrow 15but pointing in the opposite direction. This normal n, may be determinedusing principal component analysis. The eigenvector with the smallesteigenvalue may correspond to the normal n_(i). The projected distance d,of the point onto its tangent line is defined by the dot product:

d _(i)=(m _(i) −o _(i))·n _(i).  (42)

wherein point o_(i) is the point at which the normal n, begins ontangent line 14. Now a Gaussian type manifold prior can be defined ofthe form:

$\begin{matrix}{{{p_{m}(m)} = {\eta_{m}{\prod\limits_{i}\; {\exp \left\{ {- \frac{d_{i}^{2}}{2\sigma_{m}}} \right\}}}}},} & (43)\end{matrix}$

where σ_(m) is the variance of tangent line distances andη_(m)=Π_(i)(σ_(m)√{square root over (2π)})⁻¹ is a normalizer whichsubsumes all constant factors.

FIGS. 2 b and 2 c illustrate adjustments of observation points G and H,respectively, with circle 12 being re-centered on each of these points Gand H. The details of these adjustments may be substantially similar tothe adjustment of point F, and thus are not described in detail hereinin order to avoid needless repetition. Illustrated in FIG. 2 c is thecase wherein point H as observed happens to be located on or very nearto line 14. In this case, point H may not be adjusted, or may beadjusted only a very small distance.

Although the adjustments of points F, G and H are presented sequentiallyin FIGS. 2 a-c, adjustments of the individual points may be performedsimultaneously, i.e., at the same time, via use of a Conjugate Gradientoptimizer. If another optimizer is used, such as a Stochastic GradientDescent optimizer, adjustments of the individual points may be performedsequentially or randomly. This flexibility in the order of adjustmentmay be due to the adjustment of each point being performed independentlyof the adjustment of any other point. That is, each point may beadjusted based upon the original positions of the other points asactually observed, rather than being based upon the adjusted positionsof the other points. As a specific example, point G may be adjusted inFIG. 2 b based upon point F's original, unadjusted position in FIG. 2 a.However, adjustment of each point based upon the original positions ofthe other points may occur within only one iteration of theoptimization. Once all points are adjusted, the new, adjusted positionsmay be used as inputs for the next iteration.

The above-described manifold prior models the probability thatobservations belong to the same surface purely based on distance. Theshape prior, in contrast, exploits the natural local smoothness ofsurfaces. The smoothness is a function of a surface's curvature. Letm_(i) and m_(j) be neighbors on the same surface. The normal on thesurface patch is defined by

$\begin{matrix}{n_{k} = {\frac{\left( {m_{j} - m_{i}} \right)^{\bot}}{{m_{j} - m_{i}}} = {\underset{:=M_{\bot}}{\underset{}{\begin{pmatrix}0 & {- 1} \\1 & 0\end{pmatrix}}}\frac{\left( {m_{j} - m_{i}} \right)}{{m_{j} - m_{i}}}}}} & (44)\end{matrix}$

Let n_(i) and n_(ii) be two adjacent normals on the surface. Then theshape prior has the form of a sub-quadratic normal distribution

$\begin{matrix}{{p_{m}(m)} = {\frac{1}{\eta_{m}}{\prod\limits_{k}\; {\exp \left\{ {{- \frac{1}{2}}\sqrt{\left( {n_{ii} - n_{i}} \right)^{T}{\Omega_{m}\left( {n_{ii} - n_{i}} \right)}}} \right\}}}}} & (45)\end{matrix}$

Here Ω_(m) corresponds to a covariance matrix for smooth surfaces. Againp_(m) is a constant, which is the integral over all other factors andtherefore normalizes p_(m) to be a probability density function.Sub-quadratic priors are used due to their ability to enhance features.

The properties of this shape prior are demonstrated in FIGS. 3 a-c. Theprior prefers arrangements where the normals of adjacent surface patchesare similar. In fact, the properties of this prior are quite similar tothe ones of the manifold prior. The main difference is that the shape isdefined much more locally.

The shape prior technique illustrated in FIGS. 3 a-c may model the localsmoothness of natural surfaces. Two adjacent normals on a surface aremore likely to be similar, thus the distance of normals is modeled as asub-quadratic normal probabilistic distribution. The resultingdistributions are indicated by the oval areas 16 in FIGS. 3 a-c. Theseoval-shaped distributions may have a peak values in the centers of ovals16, and the values may decrease in all outward directions away from thecenters.

FIG. 3 a includes a number of circles AA-GG each of which represents arespective observation or measurement of a surface location by a robotas the robot traverses the surface. Circles AA-GG may be the result ofobservations taken in a single scan or trip of the robot across thesurface. Thus, observation points AA-GG may not have the noise in theirlocations that may result from being sensed in multiple scans. Thesurface may be substantially perpendicular to the page of FIG. 3 a, andthe robot may be displaced from the wall in the general direction ofarrow 10.

Each of observation points AA-GG may be in the same plane, as shown inFIG. 3 a. The map, the path of the robot, and the observations may bepurely two-dimensional. That is, the offsets may be in only the x and ydirection, i.e., in the plane of the page. Although each of observationpoints AA-GG may have its measured location adjusted according to theshape prior technique as described below, FIG. 3 a more particularlyillustrates an adjustment of point CC, which is arbitrarily selected forpurposes of the initial illustration.

Point CC may be moved in a direction 17 generally toward a line 18joining the two adjacent observation points BB and DD. The degree towhich, or distance by which, point CC is moved toward line 18 may bevariable according to the invention, and the invention is not intendedto be limited to any particular method of determining the amount ofmovement of point CC toward line 18. In a specific embodiment, however,point CC is moved toward line 18 by a variable amount that is governedby the sub-quadratic normal distribution represented by oval 16 andhaving peak values near line 18. Thus, the closer a possible finaldestination is to line 18, the more likely it is that point CC willactually be moved to that final destination. The greater the distancebetween point CC and line 18, the slower the drop off in distributionvalues away from line 18 may be. That is, the greater the distancebetween point CC and line 18, the flatter the distribution representedby oval 16.

FIGS. 3 b and 3 c illustrate adjustments of observation points DD andEE, respectively, with respective oval distributions 16 being disposedbetween adjacent points CC and EE in FIG. 3 b, and between adjacentpoints DD and FF in FIG. 3 c. The details of these adjustments may besubstantially similar to the adjustment of point CC, and thus are notdescribed in detail herein in order to avoid needless repetition.

Although the adjustments of points CC, DD and EE are presentedsequentially in FIGS. 3 a-c, adjustments of the individual points may beperformed simultaneously, i.e., at the same time, via use of a ConjugateGradient optimizer. If another optimizer is used, such as a StochasticGradient Descent optimizer, adjustments of the individual points may beperformed sequentially or randomly. This flexibility in the order ofadjustment may be due to the adjustment of each point being performedindependently of the adjustment of any other point. That is, each pointmay be adjusted based upon the original positions of the other points asactually observed, rather than being based upon the adjusted positionsof the other points. As specific examples, point DD may be adjusted inFIG. 3 b based upon point CC's original, unadjusted position in FIG. 3a; and point EE may be adjusted in FIG. 3 c based upon point DD'soriginal, unadjusted position in FIGS. 3 a and 3 b. However, adjustmentof each point based upon the original positions of the other points mayoccur within only one iteration of the optimization. Once all points areadjusted, the new, adjusted positions may be used as inputs for the nextiteration.

As is evident from a comparison of the manifold prior technique with theshape prior technique, the shape prior is affected more by local ornearby measurements. That is, the shape prior sub-quadratic normaldistribution may be defined by only the two closest points, while themanifold prior Gaussian distribution may be defined by a greater numberof points existing in the neighborhood of the point being adjusted.

The third prior that may be used according to the present invention isdirected to the orientation of adjacent surface patches. If two surfacepatches such as the ones shown in each of FIGS. 4 a-c belong to the samephysical surface, then the orientation of edges representing the sameportion may have a similar orientation. Once again this can be modeledas a probability distribution as follows: Let n_(i) and n_(j) be twocorresponding normals on two surface patches. Then the orientation priorhas the form of a quadratic normal distribution

$\begin{matrix}{{p_{o}(m)} = {\frac{1}{\eta_{o}}{\prod\limits_{k}\; {\exp \left\{ {{- \frac{1}{2}}\left( {n_{j} - n_{i}} \right)^{T}{\Omega_{o}\left( {n_{j} - n_{i}} \right)}} \right\}}}}} & (46)\end{matrix}$

Here Ω₀ corresponds to a covariance matrix for the orientation ofadjacent surfaces. Again η₀ is a constant, which is the integral overall other factors and therefore normalizes p₀ to be a probabilitydensity function. The resulting probability density function isrepresented by oval areas 20 in FIGS. 4 a-c.

The orientation prior illustrated in FIGS. 4 a-c uses the orientation ofadjacent surface patches. The differences between two correspondingnormals are modeled as quadratic normal distributions. The resultingdistributions are indicated by the oval areas 20 in FIGS. 4 a-c. Theseoval-shaped distributions may have a peak values in the centers of ovals20, and the values may decrease in all outward directions away from thecenters.

FIG. 4 a includes a first set of circles A₁-G₁ each of which representsa respective observation or measurement of a surface location by a robotas the robot traverses the surface in a first scan in a directiongenerally across the page of FIG. 4 a. FIG. 4 a also includes a secondset of circles A₂-F₂ each of which represents a respective observationor measurement of a surface location by a robot as the robot traversesthe surface in a second scan in a direction generally from the left-handside to the right-hand side in FIG. 4 a. Thus, observation points A₁-G₁and A₂-F₂ may not have the noise in their locations that may result frombeing sensed in multiple scans. The surface may be substantiallyperpendicular to the page of FIG. 4 a, and the robot may be displacedfrom the wall in the general direction of arrow 10.

The respective strips of surface area that are scanned to producerespective sets of observation points A₁-G₁ and A₂-F₂ may be effectivelythe same strip of surface area. However, it is possible that the sets ofobservation points A₁-G₁ and A₂-F₂ are offset relative to each other inthe x-direction across the page of FIG. 4 a.

The two sets of observation points A₁-G₁ and A₂-F₂ are shown in each ofFIGS. 4 a-c as being separated from each other by an arbitrary distancein the y-direction. However, for purposes of the orientation priortechnique, this separation distance in the y-direction is of noconsequence. The angular orientation between the two sets of observationpoints A₁-G₁ and A_(r) F₂, however, does affect the operation of theorientation prior technique. The angular orientation between the twosets of observation points A₁-G₁ and A₂-F₂ may be established by anymethod within the scope of the invention (e.g., by a least squaresmethod). However, in the illustrated embodiment, the angular orientationbetween the two sets of observation points A₁-G₁ and A₂-F₂ onceestablished may be held constant for the adjustment of each observationpoint. That is, although the first set of observation points A₁-G₁ isillustrated with different angular orientations in each of FIGS. 4 a-c,and likewise the second set of observation points A₂-F₂ is illustratedwith different angular orientations in each of FIGS. 4 a-c, theorientation of the first set of observation points A₁-G₁ relative to theorientation of the second set of observation points A₂-F₂ may be heldconstant in each of FIGS. 4 a-c and for each point adjustment.

Each of observation points A₁-G₁ and A₂-F₂ may be in the same plane, asshown in FIG. 4 a. The map, the path of the robot, and the observationsmay be purely two-dimensional. That is, the offsets may be in only the xand y directions, i.e., in the plane of the page. Although each ofobservation points A₁-G₁ may have its measured location adjustedaccording to the orientation prior technique as described below, FIG. 4a more particularly illustrates an adjustment of point C₁, which isarbitrarily selected for purposes of the initial illustration.

An adjustment of point C₁ may include selecting a line segment of thesecond set of observation points A₂-F₂ that corresponds to the linesegment that joins points B₁ and C₁. The scope of the invention mayencompass any method of selecting such a corresponding line segment.However, the corresponding line segment may be selected as one that hasthe closest position to the first line segment in the x-direction. Inthe embodiment of FIG. 4 a, the line segment joining points B₂ and C₂ isselected as the line segment corresponding to the line segment thatjoins points B₁ and C₁.

Next, an imaginary line 21 that intersects point B₁ and that is parallelto the line segment joining points B₂ and C₂ is determined. As anadjustment, point C₁ may be moved toward line 21 in a direction 22 thatis generally perpendicular to the line segment joining observationpoints B₁ and C₁. The degree to which, or distance by which, point C₁ ismoved toward line 21 may be variable according to the invention, and theinvention is not intended to be limited to any particular method ofdetermining the amount of movement of point C₁ toward line 21. In aspecific embodiment, however, point C₁ is moved toward line 21 by avariable amount that is governed by the quadratic normal distributionrepresented by oval 20 and having peak values near line 21. Thus, thecloser a possible final destination is to line 21, the more likely it isthat point C₁ will actually be moved to that final destination. Thegreater the distance between point C₁ and line 21, the slower the dropoff in distribution values away from line 21 may be. That is, thegreater the distance between point C₁ and line 21, the flatter thedistribution represented by oval 20.

FIGS. 4 b and 4 c illustrate adjustments of observation points D₁ andE₁, respectively, with respective oval distributions 20 being orientedsubstantially perpendicular to the line segment that ends at the pointbeing adjusted, and also being centered on line 21. The details of theseadjustments may be substantially similar to the adjustment of point C₁,and thus are not described in detail herein in order to avoid needlessrepetition.

Although the adjustments of points C₁, D₁ and E₁ are presentedsequentially in FIGS. 4 a-c, adjustments of the individual points may beperformed simultaneously, i.e., at the same time, via use of a ConjugateGradient optimizer. If another optimizer is used, such as a StochasticGradient Descent optimizer, adjustments of the individual points may beperformed sequentially or randomly. This flexibility in the order ofadjustment may be due to the adjustment of each point being performedindependently of the adjustment of any other point. That is, each pointmay be adjusted based upon the original positions of the other points asactually observed, rather than being based upon the adjusted positionsof the other points. As specific examples, point D₁ may be adjusted inFIG. 4 b based upon point C₁'s original, unadjusted position in FIG. 4a; and point E₁ may be adjusted in FIG. 4 c based upon point D₁'soriginal, unadjusted position in FIGS. 4 a and 4 b. However, adjustmentof each point based upon the original positions of the other points mayoccur within only one iteration of the optimization. Once all points areadjusted, the new, adjusted positions may be used as inputs for the nextiteration.

As is the case with the two sets of observation points A₁-G₁ and A₂-F₂,one set may include a greater number of points, and consequently agreater number of line segments, than the other set. If the set beingadjusted includes more points and line segments, then at least one linesegment in the other set may necessarily serve as a corresponding linesegment to more than one line segment in the first set. For example, theline segment joining points E₂ and F₂ may correspond to both the linesegment joining points E₁ and F₁ and the line segment joining points F₁and G_(I). Similarly, if the set being adjusted includes fewer pointsand line segments, then at least one line segment in the other set maynot serve as a corresponding line segment to any line segment in thefirst set.

In one embodiment, just as the first set of observation points isadjusted based upon the second set of observation points, the second setof observation points may also be adjusted based upon the first set ofobservation points. More particularly, the second set of observationpoints may be adjusted based upon the first set of points as the firstset of points were observed before their adjustment.

Creating an exact probabilistic model of all potential environments maybe infeasible and not even well defined. However, in most cases, makingsome assumptions is reasonable. For example, assuming the existence ofsmooth manifolds instead of randomly distributed surfaces may be areasonable model.

In Equation (5) is defined a novel probabilistic model for the SLAMproblem, and hereinabove are described the different components for thismodel. The problem is defined as finding the maximum a-posteriorisolution (MAP) of Equation (5). Described hereinbelow is a practicalimplementation to calculate this solution.

Since maxima of Equation (5) are unaffected by monotone transformations,the negative logarithm of this expression may be turned into a sum andthis expression may be optimized instead

$\begin{matrix}\begin{matrix}{{\hat{x}}_{1:t},{\hat{m} = {\underset{x_{1:t},m}{\arg \; \min}\; {p\left( {x_{1:t},\left. m \middle| u_{1:t} \right.,z_{1:t}} \right)}}}} \\{= {\underset{x_{1:t},m}{\arg \; \min} - {\log \; \eta} - {\log \; {p\left( x_{0} \right)}} - {\log \; {p(m)}}}} \\{{- {\sum\limits_{t}{\log \; {p\left( {\left. x_{t} \middle| x_{t - 1} \right.,u_{t}} \right)}}}}} \\{{- {\sum\limits_{t}{\sum\limits_{i}{\log \; {p\left( {\left. z_{t}^{i} \middle| x_{t} \right.,m_{i}} \right)}}}}}}\end{matrix} & (47) \\{\mspace{70mu} {= {\underset{x_{1:t},m}{\arg \; \min}{E\left( {x_{0:t},m} \right)}}}} & (48)\end{matrix}$

Finding the most probable solution reduces now to finding the globalminimum of the function E(x₀, m) which is a summation oflog-likelihoods. The term—log η is a constant normalization factor andis not relevant for minimizing E(x_(0:t), m).

The algorithm of the present invention consists of three main parts:first the motion model and the observation model from Equations (7) and(27), respectively, are used to calculate an initial estimate forx_(1:t) and m_(1:t). Next, a preconditioning is applied to improve thisestimate. Finally, a non-linear conjugate gradient variant is used tofind the parameters which minimize E(x_(0:t),m). An outline of thisalgorithm is presented below as Algorithm 1.

Algorithm 1 Calculate x _(1:t), m  1: for all constrols u_(t) do  2: x_(t) ← motion_model (u_(t), x_(t−1))  3:  for all observations z_(t)^(i) do  4:   m_(i) ← observation_model (x_(t), z_(t) ^(i))  5:  end for 6: end for  7: x_(1:t), m_(1:i) ← iterative_closest_point (x_(1:t),m_(1:i))  8: y₀ = ( x_(1:t) m_(1:i) )^(T)  9: repeat 10:  create priormodel p(m) 11:  find an α_(i) that minimizes E(y_(i) + α_(i)d_(i)) 12: update the state vector y_(i+1) = y_(i) + α_(i)d_(i) 13:  calculate thenew residual r_(i+1) = −∇ E(y_(i+1)) 14:  calculate β_(i+1) ^(FR) =(r_(i+1) ^(T)r_(i+1))/(r_(i) ^(T) r_(i)) (Fletcher-Reeves method) 15: calculate the new search direction d_(i+1) = r_(i+1) + β_(i+1)d_(i) 16.until convergence

Regarding preconditioning, objective function E(x_(0:t), m) isunfortunately non-linear and thus finding the global minimum may bedifficult. The approach of the present invention is to use a simple scanalignment algorithm prior to the optimization. In particular, the“iterative-closest-point” (ICP) algorithm is used to create an initialalignment and therefore a better staring point for the optimization.Experiments have shown that this starting point is usually sufficientlyclose to the global minimum of E that the optimization proceduredescribed below converges into the correct solution.

Finding the most probable solution and therefore the most probable pathand the most probable map is the task of finding the global minimum ofthe function E(x_(1:t), m). This minimization results in highdimensional, sparse optimization problem. The Nonlinear ConjugateGradient Method (CG) of optimization may help to find a good solution.In the present invention, a Newton-Raphson line search and theFletcher-Reeves formulation may be used to linearly combine the negativegradient of the function E(x_(0:t), m) with previous such gradients. Inevery iteration of CG, the prior model is rebuilt to ensure the bestmodel is always used at each iteration. A more detailed outline ispresented in Algorithm 1 above.

One embodiment of a robotic mapping method 500 of the present inventionis illustrated in FIG. 5. In a first step 502, a robot is scanned acrossa surface to be mapped. For example, a robot may scan a surface whilemoving substantially parallel to the surface, while moving at an anglerelative to the surface, or while moving in random directions.

In a second step 504, locations of a plurality of points on the surfaceare sensed, the sensing occurring during the scanning. For example, therobot may sense location points A-M in FIG. 2 a while scanning.

In step 506, a first sensed point location is selected. For example,sensed point location F is selected in FIG. 2 a. As other examples,locations G and H are selected in FIGS. 2 b and 2 c; respectively.

In a next step 508, a first subset of the sensed point locations thatare within a vicinity of the first sensed point location are determined.For example, in FIG. 2 a, sensed point locations C, D, E and G, H thatare within a circle 12 having point location F at the center aredetermined. However, a vicinity is not restricted to an area of acertain size within the scope of the invention. For example, a vicinityof a first sensed point location may be defined as a certain number ofother sensed point locations immediately preceding and/or following thefirst sensed point location.

Next, in step 510, a line segment that approximates the first subset ofsensed point locations is ascertained. In the embodiment of FIG. 2 a,line segment 14 is ascertained by the least squares method.

In a final step 512, the first sensed point location is represented in amap of the surface by an adjusted first sensed point location, theadjusted first sensed point location being closer to the line segmentthan is the first sensed point location. For example, sensed pointlocation F may be represented in a surface map by an adjusted locationthat is closer to line segment 14 than is location F, and that ispositioned approximately along arrow 15 in FIG. 2 a.

Another embodiment of a robotic mapping method 600 of the presentinvention is illustrated in FIG. 6. In a first step 602, a robot isscanned across a surface to be mapped. For example, a robot may scan asurface from the left-hand side to the right-hand side of FIG. 3 a.

In a second step 604, locations of a plurality of points on the surfaceare sensed, the sensing occurring during the scanning. For example, therobot may sense location points AA-GG in FIG. 3 a while scanning.

In step 606, a first sensed point location is selected. For example,sensed point location CC is selected in FIG. 3 a. As other examples,locations DD and EE are selected in FIGS. 3 b and 3 c, respectively.

In a next step 608, a preceding subset of the sensed point locations isdetermined, the preceding subset being disposed before the first sensedpoint location along a path of the scanning. In the embodiment of FIG. 3a, for example, the preceding subset includes only a single sensed pointlocation BB. Point location BB is disposed before point location CCalong the left to right scanning path in FIG. 3 a. However, it ispossible within the scope of the invention for the preceding subset toinclude a plurality of sensed point locations, such as both BB and CC,for example.

Next, in step 610, a following subset of the sensed point locations isdetermined, the following subset being disposed after the first sensedpoint location along the path of the scanning. In the embodiment of FIG.3 a, for example, the following subset includes only a single sensedpoint location DD. Point location DD is disposed after point location CCalong the left to right scanning path in FIG. 3 a. However, it ispossible within the scope of the invention for the following subset toinclude a plurality of sensed point locations, such as point locationsDD, EE and FF, for example.

In a final step 612, the first sensed point location is represented in amap of the surface by an adjusted first sensed point location, theadjusted first sensed point location being closer to each of thepreceding and following subsets of the sensed point locations than isthe first sensed point location. For example, sensed point location CCmay be represented in a surface map by an adjusted location that iscloser to line segment 18 than is location CC, and that is positionedapproximately along arrow 17 in FIG. 3 a.

Another embodiment of a robotic mapping method 700 of the presentinvention is illustrated in FIG. 7. In a first step 702, at least onerobot is scanned across a surface to be mapped, the scanning including afirst scan and a second scan. For example, in the embodiment of FIG. 4a, at least one robot is scanned in a left to right direction across asurface to be mapped. The scanning includes a first scan and a secondscarf, and these scans may be performed by the same robot or bydifferent robots.

In a second step 704, locations of a plurality of points on the surfaceare sensed, a first set of the sensed point locations being sensedduring the first scan, a second set of the sensed point locations beingsensed during the second scan. For example, in the embodiment of FIG. 4a, a first set of sensed point locations A₁-G₁ are sensed during a firstscan, and a second set of sensed point locations A₂-F₂ are sensed duringa second scan.

In step 706, a first pair of adjacent sensed point locations areselected from the first set, a first imaginary line segment joining thefirst pair of adjacent sensed point locations having a first slope. Thatis, adjacent sensed point locations B₁ and C₁ are selected from thefirst set. An imaginary line segment joining point locations B₁ and C₁is shown in FIG. 4 a, and this line segment appears to have a slope ofapproximately 0.5 in the viewpoint of FIG. 4 a.

In a next step 708, a second pair of adjacent sensed point locations isselected from the second set, the first pair of adjacent sensed pointlocations having a position in a direction of the scanning thatcorresponds to a position of the second pair of adjacent sensed pointlocations in the direction of the scanning, a second imaginary linesegment joining the second pair of adjacent sensed point locationshaving a second slope. For example, continuing with the example of FIG.4 a, a second pair of adjacent sensed point locations B₂ and C₂ areselected from the second set. The first pair of adjacent sensed pointlocations B₁ and C₁ have a position in a direction of the scanning(i.e., in the left to right direction) that corresponds to a position ofthe second pair of adjacent sensed point locations B₂ and C₂ in thedirection of the scanning. That is, locations B₁ and C₁ may beapproximately horizontally aligned with locations B₂ and C₂. Statedanother way, locations B₁ and C₁ are relatively close to locations B₂and C₂. A second imaginary line segment joins the second pair ofadjacent sensed point locations B₂ and C₂, as shown in FIG. 4 a. Thisline segment joining point locations B₂ and C₂ has a slope ofapproximately zero. That is, the line segment joining point locations B₂and C₂ is approximately horizontal in the viewpoint of FIG. 4 a.

In a final step 710, one of the first pair of adjacent sensed pointlocations is represented in a map of the surface by an adjusted firstsensed point location, a third imaginary line segment joining theadjusted first sensed point location and an other sensed point locationof the first pair, the third imaginary line segment having a thirdslope, the third slope being closer to the second slope than is thefirst slope. For example, in the embodiment of FIG. 4 a, point locationC₁ is represented in a map of the surface by an adjusted location thatis closer to the line segment joining points B₂ and C₂ than is locationC₁ and is positioned approximately along arrow 22 in FIG. 4 a. A thirdimaginary line segment may join the adjusted first sensed point locationand sensed point location B₁. This third imaginary line segment has aslope that is closer to the slope of the line segment joining points B₂and C₂ than is the slope of the line segment joining points B₁ and C₁.This third imaginary line segment has a slope that is closer to the zeroslope of the line segment joining points B₂ and C₂ (i.e., flatter) thanis the slope of the line segment joining points B₁ and C₁.

The present invention has been described herein primarily in connectionwith mapping in planar environments with two-dimensional maps and posesto be estimated. However, it is to be understood that the presentinvention is equally applicable to two-dimensional maps and poses to beestimated.

While this invention has been described as having an exemplary design,the present invention may be further modified within the spirit andscope of this disclosure. This application is therefore intended tocover any variations, uses, or adaptations of the invention using itsgeneral principles.

1. A robotic mapping method, comprising: scanning a robot across asurface to be mapped; sensing locations of a plurality of points on thesurface, the sensing occurring during the scanning; selecting a firstsaid sensed point location; determining a first subset of the sensedpoint locations that are within a vicinity of the first sensed pointlocation; ascertaining a line segment that approximates the first subsetof sensed point locations; and representing the first sensed pointlocation in a map of the surface by an adjusted first sensed pointlocation, the adjusted first sensed point location being closer to theline segment than is the first sensed point location.
 2. The method ofclaim 1 wherein the point locations are sensed in a plurality of scans.3. The method of claim 1 wherein the selecting, determining,ascertaining and representing steps are repeated for each of the sensedpoint locations other than the first sensed point location.
 4. Themethod of claim 1 wherein the vicinity of the first sensed pointlocation comprises a circular area having the first sensed pointlocation at a center of the circular area.
 5. The method of claim 1comprising the further step of calculating a probabilistic distributionassociated with the adjusted first sensed point location, theprobabilistic distribution having a peak value at the line segment andhaving values decreasing in directions substantially perpendicular tothe line segment, the representing step being dependent upon theprobabilistic distribution.
 6. A robotic mapping method, comprising:scanning a robot across a surface to be mapped; sensing locations of aplurality of points on the surface, the sensing occurring during thescanning; selecting a first said sensed point location; determining apreceding subset of the sensed point locations, the preceding subsetbeing disposed before the first sensed point location along a path ofthe scanning; determining a following subset of the sensed pointlocations, the following subset being disposed after the first sensedpoint location along the path of the scanning; and representing thefirst sensed point location in a map of the surface by an adjusted firstsensed point location, the adjusted first sensed point location beingcloser to each of the preceding and following subsets of the sensedpoint locations than is the first sensed point location.
 7. The methodof claim 6 wherein the data points are collected in a single scan. 8.The method of claim 6 wherein the selecting, determining andrepresenting steps are repeated for each of the sensed point locationsother than the first sensed point location.
 9. The method of claim 6wherein the preceding subset of the sensed point locations comprises asingle sensed point location disposed adjacent to the first sensed pointlocation, the following subset of the sensed point locations comprisingan other single sensed point location disposed adjacent to the firstsensed point location.
 10. The method of claim 6 comprising the furtherstep of calculating a probabilistic distribution associated with theadjusted first sensed point location, the probabilistic distributionhaving a peak value disposed between the preceding subset and thefollowing subset, the distribution having values decreasing in alldirections away from the peak value, the representing step beingdependent upon the probabilistic distribution.
 11. The method of claim10 wherein the probabilistic distribution comprises a sub-quadraticnormal distribution.
 12. The method of claim 6 wherein the sensed pointlocations are substantially co-planar.
 13. A robotic mapping method,comprising: scanning at least one robot across a surface to be mapped,the scanning including a first scan and a second scan; sensing locationsof a plurality of points on the surface, a first set of the sensed pointlocations being sensed during the first scan, a second set of the sensedpoint locations being sensed during the second scan; selecting a firstpair of adjacent said sensed point locations from the first set, a firstimaginary line segment joining the first pair of adjacent said sensedpoint locations having a first slope; selecting a second pair ofadjacent said sensed point locations from the second set, the first pairof adjacent sensed point locations having a position in a direction ofthe scanning that corresponds to a position of the second pair ofadjacent sensed point locations in the direction of the scanning, asecond imaginary line segment joining the second pair of adjacent saidsensed point locations having a second slope; and representing one ofthe first pair of adjacent sensed point locations in a map of thesurface by an adjusted first sensed point location, a third imaginaryline segment joining the adjusted first sensed point location and another sensed point location of the first pair, the third imaginary linesegment having a third slope, the third slope being closer to the secondslope than is the first slope.
 14. The method of claim 13 wherein theone of the first pair of adjacent sensed point locations that isadjusted follows the other sensed point location of the first pair alongthe direction of the scanning.
 15. The method of claim 13 wherein theselecting and representing steps are repeated for each of the sensedpoint locations in the first set.
 16. The method of claim 13 comprisingthe further step of calculating a probabilistic distribution associatedwith the adjusted first sensed point location, the probabilisticdistribution having a peak value disposed at a location where the thirdslope is approximately equal to the second slope, the distributionhaving values decreasing in all directions away from the peak value, therepresenting step being dependent upon the probabilistic distribution.17. The method of claim 16 wherein the probabilistic distributioncomprises a quadratic normal distribution.
 18. The method of claim 13wherein the sensed point locations in the first set and the second setare substantially co-planar.
 19. A robotic mapping method, comprising:using a robot to sense locations of a plurality of points on a surfaceto be mapped; and representing a first said sensed point location in amap of the surface by an adjusted first sensed point location, theadjusted first sensed point location being dependent upon at least asecond and a third said sensed point location.
 20. The method of claim19 wherein the robot senses the locations in a plurality of scans of therobot across the surface, at least one of the second and third sensedpoint locations being sensed in a different scan of the robot than isthe first sensed point location.
 21. The method of claim 19 comprisingthe further step of calculating a probabilistic distribution associatedwith the adjusted first sensed point location, the probabilisticdistribution being dependent upon the second and third sensed pointlocations, the representing step being dependent upon the probabilisticdistribution.